#include <fcntl.h>
#include <stdlib.h>
#include <stdio.h>
#include <termios.h>
#include <string.h>
#include <unistd.h>
#include "maestro.h"

int maestro_connect(){
	int fd = open("/dev/ttyACM0", O_RDWR | O_NOCTTY | O_NONBLOCK);
	int message[] = {161}; // 0xA1, Get Errors
	unsigned char buf[2];
	struct termios options;
	if (cfsetispeed(&options, B115200) < 0) { fprintf(stderr, "error setting baud\n"); return -1; } // set baud rates
	if (cfsetospeed(&options, B115200) < 0){ fprintf(stderr, "error setting baud\n"); return -1; }
	if (fd == -1)
	{
		fprintf(stderr, "error opening Maestro\n");
		return -1;
	}
	fprintf(stderr, "Opened Maestro\n");
	tcgetattr(fd, &options);
	options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
	options.c_oflag &= ~(ONLCR | OCRNL);
	tcsetattr(fd, TCSANOW, &options);

	if(write(fd,message,4) == EOF)
	{
		fprintf(stderr, "error writing Maestro\n");
		return -1;
	}
	fprintf(stderr, "Wrote to maestro\n");
//	usleep(1000000);
	usleep(10000);
	int bytes_read = read(fd,buf,2);


	if(bytes_read != 2)
	{
		fprintf(stderr, "error reading Maestro: %d bytes read\n", bytes_read);
		exit(1);
	}

	printf("Received: 0x%02x 0x%02x from Maestro\n",buf[0],buf[1]);


	return fd;
}

int maestro_disconnect(int fd){
	close(fd);
	return 0;
}


int setTarget(int fd, int channel, unsigned int microsecPos){
// See http://www.pololu.com/docs/0J40/5.e
	microsecPos = 4*microsecPos; // quarter microseconds
	unsigned char message[5];
	message[1] = 0x84; //132
	message[2] = channel;
	message[3] = microsecPos & 0x7F;
	message[4] = (microsecPos >> 7) & 0x7F;

//	fprintf(stderr, "Set pose Writing 0x%02x 0x%02x 0x%02x 0x%02x\n", message[1], message[2], message[3], message[4]);
	int ret = write(fd,message,5);


	if(ret == EOF)
	{
		fprintf(stderr, "error writing Maestro\n");
		return -1;
	}
//	fprintf(stderr, "setTarget written %d\n", ret);
	return 0;
}

int setSpeed(int fd, int channel, int rate){
	int message[5];
	message[1] = 0x87;
	message[2] = channel;
	message[3] = rate & 0x7F;
	message[4] = (rate >> 7) & 0x7F;
	if(write(fd,message,8) == EOF)
	{
		fprintf(stderr, "error writing Maestro\n");
		return -1;
	}
	fprintf(stderr, "setSpeed written\n");
	return 0;
}


int setAccel(int fd, int channel, int rate){
	int message[5];
	message[1] = 0x89;
	message[2] = channel;
	message[3] = rate & 0x7F;
	message[4] = (rate >> 7) & 0x7F;
	if(write(fd,message,8) == EOF)
	{
		fprintf(stderr, "error writing Maestro\n");
		return -1;
	}
	fprintf(stderr, "setAccel written\n");
	return 0;
}


int getPosition(int fd, int channel){
	unsigned char message[3];
	message[1] = 0x90;
	message[2] = channel;
	//fprintf(stderr, "Writing 0x%02x 0x%02x\n", message[1], message[2]);
	if(write(fd,message,4) == EOF)
	{
		fprintf(stderr, "error writing Maestro\n");
		return -1;
	}
	//fprintf(stderr, "getPosition written\n");
	unsigned char buf[2];
	int bytes_read = read(fd,buf,2);
	if(bytes_read != 2)
	{
		fprintf(stderr, "error reading Maestro get pose: %d bytes read\n", bytes_read);
		return -1;
	}
	int ret = (buf[1] << 8) | buf[0];
	//printf("Received: 0x%02x 0x%02x from Maestro\n",buf[0],buf[1]);
//	sscanf(buf, "%u", ret);
	return ret;
}

int getMovingState(int fd){
	int message[2];
	message[1] = 0x93;
	if(write(fd,message,2) == EOF)
	{
		fprintf(stderr, "error writing Maestro\n");
		return -1;
	}
	fprintf(stderr, "getMovingState written\n");
	usleep(1000000);
	unsigned char buf[1];
	int bytes_read = read(fd,buf,1);
	if(bytes_read != 1)
	{
		fprintf(stderr, "error reading Maestro get moving state: %d bytes read\n", bytes_read);
		return -1;
	}
	int ret = (buf[1] << 8) | buf[0];
	printf("Received: 0x%02x 0x%02x from Maestro\n",buf[0],buf[1]);
//	sscanf(buf, "%u", ret);
	return ret;
}
int getErrors(int fd){
	int message[] = {161};
	if(write(fd,message,4) == EOF)
	{
		fprintf(stderr, "error writing Maestro\n");
		return -1;
	}
	fprintf(stderr, "getErrors written\n");
	usleep(1000000);
	unsigned char buf[2];
	int bytes_read = read(fd,buf,2);
	if(bytes_read != 2)
	{
		fprintf(stderr, "error reading Maestro get errors: %d bytes read\n", bytes_read);
		return -1;
	}
	int ret = (buf[1] << 8) | buf[0];
	printf("Received: 0x%02x 0x%02x from Maestro\n",buf[0],buf[1]);
//	sscanf(buf, "%u", ret);
	return ret;
}
